#include <cstdio>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <sstream>
#include <string>

using namespace cv;

Mat cameraMatrix_left = Mat::eye(3, 3, CV_64F);
Mat cameraMatrix_right = Mat::eye(3, 3, CV_64F);
Mat distCoeffs_left = Mat::zeros(5, 1, CV_64F);
Mat distCoeffs_right = Mat::zeros(5, 1, CV_64F);
Mat R1 = Mat::eye(3, 3, CV_64F);
Mat R2 = Mat::eye(3, 3, CV_64F);
Mat P1 = Mat::eye(3, 4, CV_64F);
Mat P2 = Mat::eye(3, 4, CV_64F);
Mat t = Mat::zeros(3, 1, CV_64F);
Mat view, rview, map11, map12, map21, map22, map1_right, map2_right;

void preprocessing(Mat leftimg, Mat rightimg, Mat &processed_left,
                   Mat &processed_right) {

  Mat left_cali = leftimg;
  Mat right_cali = rightimg;

  remap(leftimg, left_cali, map11, map12, INTER_LINEAR);
  remap(rightimg, right_cali, map21, map22, INTER_LINEAR);

  // crop images
  Rect rect(100, 100, 1100, 500);
  left_cali(rect).copyTo(processed_left);
  right_cali(rect).copyTo(processed_right);
}

void preprocessing_init_ovt() {
  cameraMatrix_left.at<double>(0, 0) = 8.5101570282313037e+002;
  cameraMatrix_left.at<double>(0, 1) = 0;
  cameraMatrix_left.at<double>(0, 2) = 6.4294903241371708e+002;
  cameraMatrix_left.at<double>(1, 1) = 8.5101570282313037e+002;
  cameraMatrix_left.at<double>(1, 2) = 3.6571226535368703e+002;

  distCoeffs_left.at<double>(0, 0) = -4.5606096372169030e-001;
  distCoeffs_left.at<double>(1, 0) = 2.7762904742977162e-001;
  distCoeffs_left.at<double>(2, 0) = 0;
  distCoeffs_left.at<double>(3, 0) = 0;
  distCoeffs_left.at<double>(4, 0) = 0;

  cameraMatrix_right.at<double>(0, 0) = 8.5101570282313037e+002;
  cameraMatrix_right.at<double>(0, 1) = 0;
  cameraMatrix_right.at<double>(0, 2) = 6.565543740345672e+02;
  cameraMatrix_right.at<double>(1, 1) = 8.621817056584794e+02;
  cameraMatrix_right.at<double>(1, 2) = 3.759440491956513e+02;

  distCoeffs_right.at<double>(0, 0) = -4.6898463266179963e-001;
  distCoeffs_right.at<double>(1, 0) = 3.0846693582368073e-001;
  distCoeffs_right.at<double>(2, 0) = 0;
  distCoeffs_right.at<double>(3, 0) = 0;
  distCoeffs_right.at<double>(4, 0) = 0;

  R1.at<double>(0, 0) = 9.9494294461939714e-001;
  R1.at<double>(0, 1) = -1.2594432862665906e-003;
  R1.at<double>(0, 2) = -1.0043381280570680e-001;
  R1.at<double>(1, 0) = 6.9767945799497158e-004;
  R1.at<double>(1, 1) = 9.9998391760360128e-001;
  R1.at<double>(1, 2) = -5.6283014780683337e-003;
  R1.at<double>(2, 0) = 1.0043928611582706e-001;
  R1.at<double>(2, 1) = 5.5297682377123568e-003;
  R1.at<double>(2, 2) = 9.9492782224027698e-001;

  R2.at<double>(0, 0) = 9.9634969471966672e-001;
  R2.at<double>(0, 0) = -6.0000531804683043e-004;
  R2.at<double>(0, 0) = -8.5363492346817157e-002;
  R2.at<double>(0, 0) = 1.0772808426485060e-003;
  R2.at<double>(0, 0) = 9.9998404530537022e-001;
  R2.at<double>(0, 0) = 5.5451420805497061e-003;
  R2.at<double>(0, 0) = 8.5358803283626564e-002;
  R2.at<double>(0, 0) = -5.6168610740996786e-003;
  R2.at<double>(0, 0) = 9.9633444463877752e-001;

  P1.at<double>(0, 0) = 3.3638569978202884e+002;
  P1.at<double>(0, 1) = 0.;
  P1.at<double>(0, 2) = 8.2215689849853516e+002;
  P1.at<double>(0, 3) = 0.;
  P1.at<double>(1, 0) = 0.;
  P1.at<double>(1, 1) = 3.3638569978202884e+002;
  P1.at<double>(1, 2) = 3.7054349136352539e+002;
  P1.at<double>(1, 3) = 0.;
  P1.at<double>(2, 0) = 0.;
  P1.at<double>(2, 1) = 0.;
  P1.at<double>(2, 2) = 1.;
  P1.at<double>(2, 3) = 0.;

  P2.at<double>(0, 0) = 3.3638569978202884e+002;
  P2.at<double>(0, 1) = 0.00000000e+00;
  P2.at<double>(0, 2) = 8.2215689849853516e+002;
  P2.at<double>(0, 3) = 3.3638569978202884e+002;
  P2.at<double>(1, 0) = 0.00000000e+00;
  P2.at<double>(1, 1) = 3.3638569978202884e+002;
  P2.at<double>(1, 2) = 3.7054349136352539e+002;
  P2.at<double>(1, 3) = 0.00000000e+00;
  P2.at<double>(2, 0) = 0.00000000e+00;
  P2.at<double>(2, 1) = 0.00000000e+00;
  P2.at<double>(2, 2) = 1.00000000e+00;
  P2.at<double>(2, 3) = 0.00000000e+00;

  t.at<double>(0, 0) = -1.460798830255603e+02;
  t.at<double>(1, 0) = 0.593914751737115;
  t.at<double>(2, 0) = -3.318922412193296;

  Size imageSize;
  imageSize = Size(1280, 720);
  initUndistortRectifyMap(cameraMatrix_left, distCoeffs_left, R1, P1, imageSize,
                          CV_16SC2, map11, map12);
  initUndistortRectifyMap(cameraMatrix_right, distCoeffs_right, R2, P2,
                          imageSize, CV_16SC2, map21, map22);

  //     initUndistortRectifyMap(
  //             cameraMatrix_left, distCoeffs_left, R1, P1,
  // //                   Mat(),getOptimalNewCameraMatrix(cameraMatrix_left,
  // //                                             distCoeffs_left, imageSize,
  // //                                             1, imageSize, 0),
  //             imageSize, CV_16SC2, map1, map2);
}

void preprocessing_init() {

  cameraMatrix_left.at<double>(0, 0) = 8.4328973656730534e+02;
  cameraMatrix_left.at<double>(0, 1) = 0;
  cameraMatrix_left.at<double>(0, 2) = 6.4254722043219283e+02;
  cameraMatrix_left.at<double>(1, 1) = 8.4328973656730534e+02;
  cameraMatrix_left.at<double>(1, 2) = 3.6386957462365638e+02;

  distCoeffs_left.at<double>(0, 0) = -4.6515588267788455e-01;
  distCoeffs_left.at<double>(1, 0) = 3.0620972414356884e-01;
  distCoeffs_left.at<double>(2, 0) = 0;
  distCoeffs_left.at<double>(3, 0) = 0;
  distCoeffs_left.at<double>(4, 0) = 0;

  cameraMatrix_right.at<double>(0, 0) = 8.4328973656730534e+02;
  cameraMatrix_right.at<double>(0, 1) = 0;
  cameraMatrix_right.at<double>(0, 2) = 6.4323815624304552e+02;
  cameraMatrix_right.at<double>(1, 1) = 8.4328973656730534e+02;
  cameraMatrix_right.at<double>(1, 2) = 3.6726740191446100e+02;

  distCoeffs_right.at<double>(0, 0) = -4.5268840421376710e-01;
  distCoeffs_right.at<double>(1, 0) = 2.7581762587328940e-01;
  distCoeffs_right.at<double>(2, 0) = 0;
  distCoeffs_right.at<double>(3, 0) = 0;
  distCoeffs_right.at<double>(4, 0) = 0;

  R1.at<double>(0, 0) = 9.9633894099647735e-01;
  R1.at<double>(0, 1) = 5.0349739476422640e-04;
  R1.at<double>(0, 2) = -8.5489538216038580e-02;
  R1.at<double>(1, 0) = -2.2084767971289759e-05;
  R1.at<double>(1, 1) = 9.9998413936394459e-01;
  R1.at<double>(1, 2) = 5.6320984378919426e-03;
  R1.at<double>(2, 0) = 8.5491018044476910e-02;
  R1.at<double>(2, 1) = -5.6095909765817003e-03;
  R1.at<double>(2, 2) = 9.9632314954676948e-01;

  R2.at<double>(0, 0) = 9.9491571245090771e-01;
  R2.at<double>(0, 1) = -3.3273717283825255e-04;
  R2.at<double>(0, 2) = -1.0071054763169747e-01;
  R2.at<double>(1, 0) = -2.3458683923360693e-04;
  R2.at<double>(1, 1) = 9.9998417274985374e-01;
  R2.at<double>(1, 2) = -5.6213182444584901e-03;
  R2.at<double>(2, 0) = 1.0071082408220802e-01;
  R2.at<double>(2, 1) = 5.6163632151451067e-03;
  R2.at<double>(2, 2) = 9.9489988761529069e-01;

  P1.at<double>(0, 0) = 3.1335537037988240e+02;
  P1.at<double>(0, 1) = 0.;
  P1.at<double>(0, 2) = 8.3369155120849609e+02;
  P1.at<double>(0, 3) = 0.;
  P1.at<double>(1, 0) = 0.;
  P1.at<double>(1, 1) = 3.1335537037988240e+02;
  P1.at<double>(1, 2) = 3.7599613189697266e+02;
  P1.at<double>(1, 3) = 0.;
  P1.at<double>(2, 0) = 0.;
  P1.at<double>(2, 1) = 0.;
  P1.at<double>(2, 2) = 1.;
  P1.at<double>(2, 3) = 0.;

  P2.at<double>(0, 0) = 3.1335537037988240e+02;
  P2.at<double>(0, 1) = 0.00000000e+00;
  P2.at<double>(0, 2) = 8.3369155120849609e+02;
  P2.at<double>(0, 3) = 4.6158702867905527e+02;
  P2.at<double>(1, 0) = 0.00000000e+00;
  P2.at<double>(1, 1) = 3.1335537037988240e+02;
  P2.at<double>(1, 2) = 3.7599613189697266e+02;
  P2.at<double>(1, 3) = 0.00000000e+00;
  P2.at<double>(2, 0) = 0.00000000e+00;
  P2.at<double>(2, 1) = 0.00000000e+00;
  P2.at<double>(2, 2) = 1.00000000e+00;
  P2.at<double>(2, 3) = 0.00000000e+00;


  t.at<double>(0, 0) = -1.460798830255603e+02;
  t.at<double>(1, 0) = 0.593914751737115;
  t.at<double>(2, 0) = -3.318922412193296;


  Size imageSize;
  imageSize = Size(1280, 720);
  // stereoRectify(cameraMatrix_left, distCoeffs_left, cameraMatrix_right, distCoeffs_right,
  //               imageSize, R, T, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY, 1,
  //               imageSize, &validRoi[0], &validRoi[1]);

  initUndistortRectifyMap(cameraMatrix_left, distCoeffs_left, R1, P1, imageSize,
                          CV_16SC2, map11, map12);
  initUndistortRectifyMap(cameraMatrix_right, distCoeffs_right, R2, P2,
                          imageSize, CV_16SC2, map21, map22);

  //    initUndistortRectifyMap(
  //            cameraMatrix_left, distCoeffs_left, R2, P2,
  //            //             getOptimalNewCameraMatrix(cameraMatrix_left,
  //            //                                       distCoeffs_left,
  //            imageSize,
  //            //                                       1, imageSize, 0),
  //            imageSize, CV_16SC2, map11, map12);
}

void preprocessing_init_matlab() {
  cameraMatrix_left.at<double>(0, 0) = 8.536277779716801e+02;
  cameraMatrix_left.at<double>(0, 1) = 0;
  cameraMatrix_left.at<double>(0, 2) = 6.444638969444883e+02;
  cameraMatrix_left.at<double>(1, 1) = 8.549405450154125e+02;
  cameraMatrix_left.at<double>(1, 2) = 3.661401409591311e+02;

  distCoeffs_left.at<double>(0, 0) = -0.428175337824240;
  distCoeffs_left.at<double>(1, 0) = 0.160527444134967;
  distCoeffs_left.at<double>(2, 0) = -9.393799453024958e-04;
  distCoeffs_left.at<double>(3, 0) = -8.392382304682925e-05;
  distCoeffs_left.at<double>(4, 0) = 0;

  cameraMatrix_right.at<double>(0, 0) = 8.608239509614740e+02;
  cameraMatrix_right.at<double>(0, 1) = 0;
  cameraMatrix_right.at<double>(0, 2) = 6.565543740345672e+02;
  cameraMatrix_right.at<double>(1, 1) = 8.621817056584794e+02;
  cameraMatrix_right.at<double>(1, 2) = 3.759440491956513e+02;

  distCoeffs_right.at<double>(0, 0) = -0.419259062588465;
  distCoeffs_right.at<double>(1, 0) = 0.147659131418243;
  distCoeffs_right.at<double>(2, 0) = -0.001268319518672;
  distCoeffs_right.at<double>(3, 0) = -0.001637949619228;
  distCoeffs_right.at<double>(4, 0) = 0;
  R1.at<double>(0, 0) = 0.99968617;
  R1.at<double>(0, 1) = -0.0038575;
  R1.at<double>(0, 2) = 0.02475248;
  R1.at<double>(1, 0) = 0.00381067;
  R1.at<double>(1, 1) = 0.99999086;
  R1.at<double>(1, 2) = 0.00193913;
  R1.at<double>(2, 0) = -0.02475973;
  R1.at<double>(2, 1) = -0.0018442;
  R1.at<double>(2, 2) = 0.99969173;

  R2.at<double>(0, 0) = 0.99973374;
  R2.at<double>(0, 1) = -0.0040646;
  R2.at<double>(0, 2) = 0.02271386;
  R2.at<double>(1, 0) = 0.00410757;
  R2.at<double>(1, 1) = 0.99998986;
  R2.at<double>(1, 2) = -0.00184528;
  R2.at<double>(2, 0) = -0.02270613;
  R2.at<double>(2, 1) = 0.00193809;
  R2.at<double>(2, 2) = 0.9997403;

  P1.at<double>(0, 0) = 584.89586939;
  P1.at<double>(0, 1) = 0.;
  P1.at<double>(0, 2) = 612.06429291;
  P1.at<double>(0, 3) = 0.;
  P1.at<double>(1, 0) = 0.;
  P1.at<double>(1, 1) = 584.89586939;
  P1.at<double>(1, 2) = 363.79812622;
  P1.at<double>(1, 3) = 0.;
  P1.at<double>(2, 0) = 0.;
  P1.at<double>(2, 1) = 0.;
  P1.at<double>(2, 2) = 1.;
  P1.at<double>(2, 3) = 0.;

  P2.at<double>(0, 0) = 5.84895869e+02;
  P2.at<double>(0, 1) = 0.00000000e+00;
  P2.at<double>(0, 2) = 6.12064293e+02;
  P2.at<double>(0, 3) = 8.54642755e+04;
  P2.at<double>(1, 0) = 0.00000000e+00;
  P2.at<double>(1, 1) = 5.84895869e+02;
  P2.at<double>(1, 2) = 3.63798126e+02;
  P2.at<double>(1, 3) = 0.00000000e+00;
  P2.at<double>(2, 0) = 0.00000000e+00;
  P2.at<double>(2, 1) = 0.00000000e+00;
  P2.at<double>(2, 2) = 1.00000000e+00;
  P2.at<double>(2, 3) = 0.00000000e+00;
  //    R1.at<double>(0,0) = 0.999997846164199;
  //    R1.at<double>(0,1) = 2.929280324447512e-04;
  //    R1.at<double>(0,2) = 0.002054716557376;
  //    R1.at<double>(1,0) = -3.006851795150384e-04;
  //    R1.at<double>(1,1) = 0.999992825696505;
  //    R1.at<double>(1,2) = 0.003775995755004;
  //    R1.at<double>(2,0) = -0.002053595721209;
  //    R1.at<double>(2,1) = -0.003776605444946;
  //    R1.at<double>(2,2) = 0.999990759955274;
  //
  t.at<double>(0, 0) = -1.460798830255603e+02;
  t.at<double>(1, 0) = 0.593914751737115;
  t.at<double>(2, 0) = -3.318922412193296;
  //
  //   R2.at<double>(0, 0) = 9.9993554974225540e-001;
  //   R2.at<double>(0, 1) = -1.1353240516853448e-002;
  //   R2.at<double>(0, 2) = 1.7071025185769282e-005;
  //   R2.at<double>(1, 0) = 1.1352320127896399e-002;
  //   R2.at<double>(1, 1) = 9.9987150472689235e-001;
  //   R2.at<double>(1, 2) = 1.1318076819581852e-002;
  //   R2.at<double>(2, 0) = -1.4556567996065837e-004;
  //   R2.at<double>(2, 1) = -1.1317153570870837e-002;
  //   R2.at<double>(2, 2) = 9.9993594837153754e-001;
  //
  //   P1.at<double>(0, 0) = 3.6961335849850263e+002;
  //   P1.at<double>(0, 1) = 0;
  //   P1.at<double>(0, 2) = 7.1468644714355469e+002;
  //   P1.at<double>(0, 3) = 0;
  //   P1.at<double>(1, 0) = 0;
  //   P1.at<double>(1, 1) = 3.6961335849850263e+002;
  //   P1.at<double>(1, 2) = 3.6512691879272461e+002;
  //   P1.at<double>(1, 3) = 0;
  //   P1.at<double>(2, 0) = 0;
  //   P1.at<double>(2, 1) = 0;
  //   P1.at<double>(2, 2) = 1;
  //   P1.at<double>(2, 3) = 0;
  //
  //   P2.at<double>(0, 0) = 3.6961335849850263e+002;
  //   P2.at<double>(0, 1) = 0;
  //   P2.at<double>(0, 2) = 7.1468644714355469e+002;
  //   P2.at<double>(0, 3) = -5.5476432245292839e+004;
  //   P2.at<double>(1, 0) = 0;
  //   P2.at<double>(1, 1) = 3.6961335849850263e+002;
  //   P2.at<double>(1, 2) = 3.6512691879272461e+002;
  //   P2.at<double>(1, 3) = 0;
  //   P2.at<double>(2, 0) = 0;
  //   P2.at<double>(2, 1) = 0;
  //   P2.at<double>(2, 2) = 1;
  //   P2.at<double>(2, 3) = 0;

  Size imageSize;
  imageSize = Size(1280, 720);

  initUndistortRectifyMap(cameraMatrix_left, distCoeffs_left, R1, P1, imageSize,
                          CV_16SC2, map11, map12);
  initUndistortRectifyMap(cameraMatrix_right, distCoeffs_right, R2, P2,
                          imageSize, CV_16SC2, map21, map22);

  //    initUndistortRectifyMap(
  //            cameraMatrix_left, distCoeffs_left, R2, P2,
  //            //             getOptimalNewCameraMatrix(cameraMatrix_left,
  //            //                                       distCoeffs_left,
  //            imageSize,
  //            //                                       1, imageSize, 0),
  //            imageSize, CV_16SC2, map11, map12);
}

int main() {
  preprocessing_init();

  int count = 0;
  for (int i = 0; i < 41; i++) {

    std::stringstream ss_left, ss_right;
    ss_left.clear();
    ss_left << "/home/bobin/bak/hobot/stereo/data/pair_2_bak_bak/left/"
            << i * 100 << ".jpg";

    std::cout << ss_left.str() << std::endl;
    ss_right.clear();
    ss_right << "/home/bobin/bak/hobot/stereo/data/pair_2_bak_bak/right/"
             << i * 100 << ".jpg";
    std::cout << ss_right.str() << std::endl;
    cv::Mat left_gray = cv::imread(ss_left.str());
    cv::Mat right_gray = cv::imread(ss_right.str());

    if (left_gray.empty() || right_gray.empty())
      continue;

    Mat right_roi = right_gray;
    Mat left_roi = left_gray;

    preprocessing(left_gray, right_gray, left_roi, right_roi);

    cv::Mat im_show(left_roi.rows, 2 * left_roi.cols, CV_8UC3);
    left_roi.copyTo(im_show.colRange(0, left_roi.cols));
    right_roi.copyTo(im_show.colRange(left_roi.cols, 2 * left_roi.cols));

    for (int i = 0; i <= 10; i++) {
      line(im_show, Point(0, 50 * i), Point(im_show.cols, 50 * i),
           Scalar(255, 255, 255));
    }

    cv::imshow("calib", im_show);
    cv::waitKey(0);
  }
}
